#include "ultra_band_wave_ladar/Sensor_Ultra.hpp"

Sensor_Ultra::Sensor_Ultra()
{
  m_alarmDistance = 0;
  act_distance = 0;
  act_amp = 0;
  act_angle = 0;
}

Sensor_Ultra::~Sensor_Ultra()
{
}

void Sensor_Ultra::set_AlarmDistance(int Dist)
{
  // unit cm
  m_alarmDistance = Dist;
}

bool Sensor_Ultra::Init_UltraRadComPort(std::string name)
{
  try
  {
    ser.setPort(name);
    ser.setBaudrate(115200);  //默认115200
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (const std::exception& e)
  {
    ROS_ERROR_STREAM("Unbale to open port");
    return false;
  }

  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port  Initialized");
  }
  else
  {
    return false;
  }
  return true;
}

void Sensor_Ultra::setScanData(std::vector<uint8_t> data)
{
  for (int i = 0; i < data.size(); i++)
  {
    reciveData.push_back(data[i]);
  }
  act_distance = comb16(reciveData[1], reciveData[2]);
  // act_amp = comb16(reciveData[3],reciveData[4]);
  // act_angle = data;
}

//激活雷达,开始扫描
void Sensor_Ultra::UltraRadScan_Start()
{
  set_CommandMode(0, 0);
}

uint16_t Sensor_Ultra::comb16(uint8_t low, uint8_t high)
{
  uint16_t newdata = 0;
  newdata = ((uint16_t)high << 8);
  newdata |= ((uint16_t)low);
}

void Sensor_Ultra::set_CommandMode(int type, int Var)
{
  uint8_t Temp = 0x00;
  switch (type)
  {
    case 0:
      break;
    default:
      break;
  }
}

void Sensor_Ultra::CloseComPort()
{
  ser.close();
}

uint8_t Sensor_Ultra::DataOXROperate(uint8_t* pt, int num)
{
  uint8_t Data = 0x00;
  for (int i = 1; i < num + 1; i++)
  {
    Data = Data ^ pt[i];
  }
  return Data;
}

void Sensor_Ultra::get_UltraRadComScanData()
{
  size_t n = ser.available();
  if (n)
  {
    ROS_INFO_STREAM("Reading from Ultra serial port\n");
    uint8_t buffer[1024];
    n = ser.read(buffer, n);
    for (int i = 0; i < n; i++)
    {
      std::cout << std::hex << (buffer[i] & 0xff) << " ";
    }
    act_distance = comb16(buffer[1], buffer[2]);
    // act_amp = comb16(buffer[3],buffer[4]);
    // mmRad.mag = comb16(buffer[3],buffer[4]);
    // std::cout<<"Obstacle's position is "<<distance<<" cm"<<" ";
    // std::cout<<"Obstacle's magnitude is "<<mag<<" ";
    // std::cout<<std::endl;
  }
}
uint16_t Sensor_Ultra::get_ActualDistance()
{
  return act_distance;
}
uint16_t Sensor_Ultra::get_ActualAmplitude()
{
  return act_amp;
}
uint16_t Sensor_Ultra::get_ActualAngle()
{
  return act_amp;
}
bool Sensor_Ultra::ObstacleScan()
{
  std_msgs::String result;
  if (act_distance != 0 && act_distance <= m_alarmDistance)
  {
    result.data = "Obstacle's distance:" + std::to_string(act_distance);
    ROS_INFO_STREAM("mmRad_Sensor:" << result.data);
    return true;
  }
  return false;
}